#%%
import numpy as np
import matplotlib.pyplot as plt

imutype = np.dtype([('head', 'i4'),
                    ('gyrox', 'f4'), ('gyroy', 'f4'),
                    ('gyroz', 'f4'), ('accelx', 'f4'), ('accely', 'f4'),
                    ('accelz', 'f4')])

data = np.fromfile("/home/nuc/workspace/ai-robot-plus/catkin_ws/data/A15_2022_6_1_13_49_40.bin", dtype=imutype)

dt = np.diff(data['head'])
plt.plot(dt)
plt.title("delta")
plt.show()

plt.plot(data['head'], data['gyrox'])
plt.plot(data['head'], data['gyroy'])
plt.plot(data['head'], data['gyroz'])
plt.ylabel("deg")
plt.title("gyro increment")
plt.show()

plt.plot(data['head'], data['accelx'])
plt.plot(data['head'], data['accely'])
plt.plot(data['head'], data['accelz'])
plt.ylabel("m/s")
plt.title("accel increment")
plt.show()
